#ifndef HERO_VISION__TARGET_MANAGER_HPP_
#define HERO_VISION__TARGET_MANAGER_HPP_

#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <rclcpp/rclcpp.hpp>
#include <memory>
#include <string>
#include <vector>

#include <sensor_msgs/msg/camera_info.hpp>

// 自定义依赖
#include "hero_integrated_vision/armor.hpp"
#include "aim_interfaces/msg/armors.hpp"
#include "aim_interfaces/msg/gimbal_control.hpp"
#include "hero_integrated_vision/pnp_solver.hpp"

namespace rm_aim
{

class HeroVisionNode;

class CameraParamsManager
{
public:
    explicit CameraParamsManager(HeroVisionNode* node);

    /**
     * @brief 核心接口：传入检测结果，自动根据GimbalControl指令生成消息，用于普通自瞄/前哨站
     * @param armors 检测到的常规装甲板
     * @param img_header 图像消息头（时间戳对齐）
     * @return 可直接发布的Armors消息
     */
    aim_interfaces::msg::Armors generateArmorsMsg(
        const std::vector<Armor>& armors,
        const std_msgs::msg::Header& img_header);
    
    /**
     * @brief 控制指令回调（use_scope）
     * @param use_scope 是否使用倍镜模式
     */
    void updateScopeEnableState(const bool & use_scope )
    {
        is_scope_enabled_ = use_scope;
    }

private:
    // 1. 内参结构体
    struct CameraParams
    {
        std::unique_ptr<PnPSolver> pnp_solver;  // PnP求解器
        cv::Point2f cam_center;                 // 图像中心
        std::shared_ptr<sensor_msgs::msg::CameraInfo> cam_info;  // 内参缓存
        bool is_initialized = false;            // 初始化标记
    };

    // 2. 节点关联
    HeroVisionNode* node_;
    rclcpp::Logger logger_;

    // 3. 两套内参
    CameraParams general_params_;    // 第一套：非倍镜（normal_aim/outpost默认）
    CameraParams scope_params_; // 第二套：倍镜（use_scope=true时启用）

    // 4. 电控控制指令缓存（核心：use_scope）
    bool is_scope_enabled_ = false;  // 倍镜模式

    // 5. 内参订阅者（收到一次销毁）
    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr general_cam_sub_;
    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr scope_cam_sub_;

    // 6. 消息缓存
    aim_interfaces::msg::Armors armors_msg_;

    // -------------------------- 内部辅助函数 --------------------------
    /**
     * @brief 订阅两套内参
     */
    void subscribeCameraParams();

    /**
     * @brief 根据use_scope选择内参（核心：内参选择逻辑）
     * @return 选中的内参结构体（返回引用，避免拷贝）
     */
    const CameraParams& selectCameraParams() const;

    /**
     * @brief 处理常规装甲板（普通自瞄/前哨站模式）
     */
    void processArmors(
        const std::vector<Armor>& armors,
        const CameraParams& selected_params);

    /**
     * @brief 处理绿灯（绿灯辅助模式）
     */
    void processGreenLights(
        const std::vector<cv::Rect2f>& green_lights,
        const CameraParams& selected_params);

    /**
     * @brief 提取绿灯角点
     */
    std::vector<cv::Point2f> getGreenLightCorners(const cv::Rect2f& rect);
};

}  // namespace rm_aim

#endif 